#include "featureTest.h"
#include "cvxImage.h"
#include <time.h>
#include "cvxMatch.h"

void FeatureTest::testSURF()
{
	string fA = string("image\\left0.png");  //left0_small
	string fB = string("image\\right0.png"); //right0_small


	Mat img1 = imread(fA.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(fB.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	if(img1.empty() || img2.empty())
	{
		printf("Can't read one of the images\n");
		return ;
	}
	printf("image width = %d height = %d\n", img1.cols, img2.rows);

	double tt = clock();
	// detecting keypoints
	SurfFeatureDetector detector(8000);
	vector<KeyPoint> keypoints1, keypoints2;
	detector.detect(img1, keypoints1);
	detector.detect(img2, keypoints2);

	printf("feature detect cost time = %f\n", clock() - tt);
	tt = clock();

	// computing descriptors
	SurfDescriptorExtractor extractor;
	Mat descriptors1, descriptors2;
	extractor.compute(img1, keypoints1, descriptors1);
	extractor.compute(img2, keypoints2, descriptors2);

	printf("extract cost time = %f\n", clock() - tt);
	tt = clock();

	// matching descriptors
	BFMatcher matcher(NORM_L2);
	vector<DMatch> matches;
	matcher.match(descriptors1, descriptors2, matches);

	vector<Point2f> pts1;
	vector<Point2f> pts2;
	for (int i = 0; i<matches.size(); ++i)
	{
		int i1 = matches[i].queryIdx;
		int i2 = matches[i].trainIdx;
		pts1.push_back(keypoints1[i1].pt);
		pts2.push_back(keypoints2[i2].pt);
	}

	Mat inlierMat;
	Mat F = cv::findFundamentalMat(pts1, pts2, FM_RANSAC, 1, 0.99, inlierMat);

	vector<DMatch> matchInlier;
	for (int i = 0; i<pts1.size(); ++i)
	{
		if (inlierMat.at<unsigned char>(i, 0) != 0)
		{
			matchInlier.push_back(matches[i]);							
		}
	}
	printf("match cost time = %f\n", clock() - tt);
	tt = clock();

	// drawing the results
	namedWindow("matches", 1);
	Mat img_matches;
	drawMatches(img1, keypoints1, img2, keypoints2, matchInlier, img_matches);
	imshow("matches", img_matches);
	waitKey(0);
	return ;
}

void FeatureTest::testFAST()
{
	string fA = string("image\\left0.png");  //left0_small
	string fB = string("image\\right0.png"); //right0_small


	Mat img1 = imread(fA.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(fB.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	if(img1.empty() || img2.empty())
	{
		printf("Can't read one of the images\n");
		return ;
	}
	printf("image width = %d height = %d\n", img1.cols, img2.rows);

	double tt = clock();
	// detecting keypoints
	FastFeatureDetector detector(50);
	vector<KeyPoint> keypoints1, keypoints2;
	detector.detect(img1, keypoints1);
	detector.detect(img2, keypoints2);

	printf("feature detect cost time = %f\n", clock() - tt);
	tt = clock();

	// computing descriptors
	BriefDescriptorExtractor extractor(16);
	Mat descriptors1, descriptors2;
	extractor.compute(img1, keypoints1, descriptors1);
	extractor.compute(img2, keypoints2, descriptors2);

	printf("extract cost time = %f\n", clock() - tt);
	tt = clock();

	// matching descriptors
	BFMatcher matcher(NORM_HAMMING);
	vector<DMatch> matches;
	
	matcher.match(descriptors1, descriptors2, matches);

	printf("match cost time = %f\n", clock() - tt);
	tt = clock();
	vector<Point2f> pts1;
	vector<Point2f> pts2;
	for (int i = 0; i<matches.size(); ++i)
	{
		int i1 = matches[i].queryIdx;
		int i2 = matches[i].trainIdx;
		pts1.push_back(keypoints1[i1].pt);
		pts2.push_back(keypoints2[i2].pt);
	}

	Mat inlierMat;
	Mat F = cv::findFundamentalMat(pts1, pts2, FM_RANSAC, 1, 0.99, inlierMat);

	vector<DMatch> matchInlier;
	for (int i = 0; i<pts1.size(); ++i)
	{
		if (inlierMat.at<unsigned char>(i, 0) != 0)
		{
			matchInlier.push_back(matches[i]);							
		}
	}
	printf("inlier cost time = %f\n", clock() - tt);
	tt = clock();
	printf("match numbers = %d\n", pts1.size());

	// drawing the results
	namedWindow("matches", 1);
	Mat img_matches;
	drawMatches(img1, keypoints1, img2, keypoints2, matchInlier, img_matches);
	imshow("matches", img_matches);
	waitKey(0);
	return ;

//	GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
}

void FeatureTest::testGridFAST()
{
	string fA = string("image\\left0.png");  //left0_small
	string fB = string("image\\right0.png"); //right0_small


	Mat img1 = imread(fA.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(fB.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	if(img1.empty() || img2.empty())
	{
		printf("Can't read one of the images\n");
		return ;
	}
	printf("image width = %d height = %d\n", img1.cols, img2.rows);

	double tt = clock();
	// detecting keypoints
//	FastFeatureDetector detector(50);
	int maxTotalPts = 1000;
	GridAdaptedFeatureDetector detector(new FastFeatureDetector(50, true), maxTotalPts, 4, 4);
	vector<KeyPoint> keypoints1, keypoints2;
	detector.detect(img1, keypoints1);
	detector.detect(img2, keypoints2);

	printf("key point number is %d %d\n", keypoints1.size(), keypoints2.size());
	printf("feature detect cost time = %f\n", clock() - tt);
	tt = clock();

	// computing descriptors
	BriefDescriptorExtractor extractor(16);
	Mat descriptors1, descriptors2;
	extractor.compute(img1, keypoints1, descriptors1);
	extractor.compute(img2, keypoints2, descriptors2);

	printf("extract cost time = %f\n", clock() - tt);
	tt = clock();

	// matching descriptors
	BFMatcher matcher(NORM_HAMMING);
	vector<DMatch> matches;

	matcher.match(descriptors1, descriptors2, matches);

	printf("match cost time = %f\n", clock() - tt);
	tt = clock();
	vector<Point2f> pts1;
	vector<Point2f> pts2;
	for (int i = 0; i<matches.size(); ++i)
	{
		int i1 = matches[i].queryIdx;
		int i2 = matches[i].trainIdx;
		pts1.push_back(keypoints1[i1].pt);
		pts2.push_back(keypoints2[i2].pt);
	}

	Mat inlierMat;
	Mat F = cv::findFundamentalMat(pts1, pts2, FM_RANSAC, 1, 0.99, inlierMat);

	vector<DMatch> matchInlier;
	for (int i = 0; i<pts1.size(); ++i)
	{
		if (inlierMat.at<unsigned char>(i, 0) != 0)
		{
			matchInlier.push_back(matches[i]);							
		}
	}
	printf("inlier cost time = %f\n", clock() - tt);
	tt = clock();
	printf("match numbers = %d\n", pts1.size());

	// drawing the results
	namedWindow("matches", 1);
	Mat img_matches;
	drawMatches(img1, keypoints1, img2, keypoints2, matchInlier, img_matches);
	imshow("matches", img_matches);
	imwrite("matches.png", img_matches);
	
	waitKey(0);
	return ;
}

void FeatureTest::testDevideGridFast()
{
	string fA = string("image\\left0.png");  //left0_small
	string fB = string("image\\right0.png"); //right0_small


	Mat img1 = imread(fA.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(fB.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	if(img1.empty() || img2.empty())
	{
		printf("Can't read one of the images\n");
		return ;
	}
	printf("image width = %d height = %d\n", img1.cols, img2.rows);

	double tt = clock();
	// detecting keypoints
	//	FastFeatureDetector detector(50);
	int maxTotalPts = 500;
	GridAdaptedFeatureDetector detector(new FastFeatureDetector(50, true), maxTotalPts, 4, 4);
	vector<KeyPoint> keypoints1, keypoints2;
	detector.detect(img1, keypoints1);
	detector.detect(img2, keypoints2);

	printf("key point number is %d %d\n", keypoints1.size(), keypoints2.size());
//	printf("feature detect cost time = %f\n", clock() - tt);
//	tt = clock();

	
	// computing descriptors
	BriefDescriptorExtractor extractor(16);
	Mat descriptors1, descriptors2;
	extractor.compute(img1, keypoints1, descriptors1);
	extractor.compute(img2, keypoints2, descriptors2);

//	printf("extract cost time = %f\n", clock() - tt);
//	tt = clock();

	// matching descriptors
	BFMatcher matcher(NORM_HAMMING);
	vector<DMatch> matches;

	matcher.match(descriptors1, descriptors2, matches);

//	printf("match cost time = %f\n", clock() - tt);
//	tt = clock();
	vector<Point2f> pts1;
	vector<Point2f> pts2;
	for (int i = 0; i<matches.size(); ++i)
	{
		int i1 = matches[i].queryIdx;
		int i2 = matches[i].trainIdx;
		pts1.push_back(keypoints1[i1].pt);
		pts2.push_back(keypoints2[i2].pt);
	}

	Mat inlierMat;
	Mat F = cv::findFundamentalMat(pts1, pts2, FM_RANSAC, 1, 0.99, inlierMat);

	vector<DMatch> matchInlier;
	for (int i = 0; i<pts1.size(); ++i)
	{
		if (inlierMat.at<unsigned char>(i, 0) != 0)
		{
			matchInlier.push_back(matches[i]);							
		}
	}
	printf("total cost time = %f\n", clock() - tt);
	tt = clock();
	printf("match numbers = %d -> %d\n", pts1.size(), matchInlier.size());

	// drawing the results
	namedWindow("matches", 1);
	Mat img_matches;
	drawMatches(img1, keypoints1, img2, keypoints2, matchInlier, img_matches);
	imshow("matches", img_matches);
	imwrite("matches.png", img_matches);

	waitKey(0);
	return ;
}

void FeatureTest::testAdaptiveVideo()
{
	string fA = string("image\\left0.png");  //left0_small
	string fB = string("image\\right0.png"); //right0_small


	Mat img1 = imread(fA.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(fB.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	if(img1.empty() || img2.empty())
	{
		printf("Can't read one of the images\n");
		return ;
	}

	CvxMatch myMatch(50, 150);
	vector<Point2f> ptA;
	vector<Point2f> ptB;
	
	double tt = clock();
	myMatch.match(img1, img2, ptA, ptB);
	printf("cost time = %f\n", clock() - tt);

}